/*
 * Copyright (c) 2013, Oracle and/or its affiliates. All rights reserved.
 * DO NOT ALTER OR REMOVE COPYRIGHT NOTICES OR THIS FILE HEADER.
 *
 * This code is free software; you can redistribute it and/or modify it
 * under the terms of the GNU General Public License version 2 only, as
 * published by the Free Software Foundation.  Oracle designates this
 * particular file as subject to the "Classpath" exception as provided
 * by Oracle in the LICENSE file that accompanied this code.
 *
 * This code is distributed in the hope that it will be useful, but WITHOUT
 * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
 * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License
 * version 2 for more details (a copy is included in the LICENSE file that
 * accompanied this code).
 *
 * You should have received a copy of the GNU General Public License version
 * 2 along with this work; if not, write to the Free Software Foundation,
 * Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA.
 *
 * Please contact Oracle, 500 Oracle Parkway, Redwood Shores, CA 94065 USA
 * or visit www.oracle.com if you need additional information or have any
 * questions.
 */

package com.javafx.experiments.dukepad.compass.imu;

import java.nio.ByteBuffer;
import java.nio.ByteOrder;
import java.util.Arrays;

public class InvMpuDmpMotionDriver {
    public static final short TAP_X               = 0x01;
    public static final short TAP_Y               = 0x02;
    public static final short TAP_Z               = 0x04;
    public static final short TAP_XYZ             = 0x07;

    public static final short TAP_X_UP            = 0x01;
    public static final short TAP_X_DOWN          = 0x02;
    public static final short TAP_Y_UP            = 0x03;
    public static final short TAP_Y_DOWN          = 0x04;
    public static final short TAP_Z_UP            = 0x05;
    public static final short TAP_Z_DOWN          = 0x06;

    public static final short ANDROID_ORIENT_PORTRAIT             = 0x00;
    public static final short ANDROID_ORIENT_LANDSCAPE            = 0x01;
    public static final short ANDROID_ORIENT_REVERSE_PORTRAIT     = 0x02;
    public static final short ANDROID_ORIENT_REVERSE_LANDSCAPE    = 0x03;

    public static final short DMP_INT_GESTURE     = 0x01;
    public static final short DMP_INT_CONTINUOUS  = 0x02;

    public static final int DMP_FEATURE_TAP             = 0x001;
    public static final int DMP_FEATURE_ANDROID_ORIENT  = 0x002;
    public static final int DMP_FEATURE_LP_QUAT         = 0x004;
    public static final int DMP_FEATURE_PEDOMETER       = 0x008;
    public static final int DMP_FEATURE_6X_LP_QUAT      = 0x010;
    public static final int DMP_FEATURE_GYRO_CAL        = 0x020;
    public static final int DMP_FEATURE_SEND_RAW_ACCEL  = 0x040;
    public static final int DMP_FEATURE_SEND_RAW_GYRO   = 0x080;
    public static final int DMP_FEATURE_SEND_CAL_GYRO   = 0x100;

    public static final int INV_WXYZ_QUAT       = 0x100;

    /* These defines are copied from dmpDefaultMPU6050.c in the general MPL
     * releases. These defines may change for each DMP image, so be sure to modify
     * these values when switching to a new image.
     */
    private static final int CFG_LP_QUAT             = 2712;
    private static final int END_ORIENT_TEMP         = 1866;
    private static final int CFG_27                  = 2742;
    private static final int CFG_20                  = 2224;
    private static final int CFG_23                  = 2745;
    private static final int CFG_FIFO_ON_EVENT       = 2690;
    private static final int END_PREDICTION_UPDATE   = 1761;
    private static final int CGNOTICE_INTR           = 2620;
    private static final int X_GRT_Y_TMP             = 1358;
    private static final int CFG_DR_INT              = 1029;
    private static final int CFG_AUTH                = 1035;
    private static final int UPDATE_PROP_ROT         = 1835;
    private static final int END_COMPARE_Y_X_TMP2    = 1455;
    private static final int SKIP_X_GRT_Y_TMP        = 1359;
    private static final int SKIP_END_COMPARE        = 1435;
    private static final int FCFG_3                  = 1088;
    private static final int FCFG_2                  = 1066;
    private static final int FCFG_1                  = 1062;
    private static final int END_COMPARE_Y_X_TMP3    = 1434;
    private static final int FCFG_7                  = 1073;
    private static final int FCFG_6                  = 1106;
    private static final int FLAT_STATE_END          = 1713;
    private static final int SWING_END_4             = 1616;
    private static final int SWING_END_2             = 1565;
    private static final int SWING_END_3             = 1587;
    private static final int SWING_END_1             = 1550;
    private static final int CFG_8                   = 2718;
    private static final int CFG_15                  = 2727;
    private static final int CFG_16                  = 2746;
    private static final int CFG_EXT_GYRO_BIAS       = 1189;
    private static final int END_COMPARE_Y_X_TMP     = 1407;
    private static final int DO_NOT_UPDATE_PROP_ROT  = 1839;
    private static final int CFG_7                   = 1205;
    private static final int FLAT_STATE_END_TEMP     = 1683;
    private static final int END_COMPARE_Y_X         = 1484;
    private static final int SKIP_SWING_END_1        = 1551;
    private static final int SKIP_SWING_END_3        = 1588;
    private static final int SKIP_SWING_END_2        = 1566;
    private static final int TILTG75_START           = 1672;
    private static final int CFG_6                   = 2753;
    private static final int TILTL75_END             = 1669;
    private static final int END_ORIENT              = 1884;
    private static final int CFG_FLICK_IN            = 2573;
    private static final int TILTL75_START           = 1643;
    private static final int CFG_MOTION_BIAS         = 1208;
    private static final int X_GRT_Y                 = 1408;
    private static final int TEMPLABEL               = 2324;
    private static final int CFG_ANDROID_ORIENT_INT  = 1853;
    private static final int CFG_GYRO_RAW_DATA       = 2722;
    private static final int X_GRT_Y_TMP2            = 1379;

    private static final int D_0_22                  = 22+512;
    private static final int D_0_24                  = 24+512;

    private static final int D_0_36                  = 36;
    private static final int D_0_52                  = 52;
    private static final int D_0_96                  = 96;
    private static final int D_0_104                 = 104;
    private static final int D_0_108                 = 108;
    private static final int D_0_163                 = 163;
    private static final int D_0_188                 = 188;
    private static final int D_0_192                 = 192;
    private static final int D_0_224                 = 224;
    private static final int D_0_228                 = 228;
    private static final int D_0_232                 = 232;
    private static final int D_0_236                 = 236;

    private static final int D_1_2                   = 256 + 2;
    private static final int D_1_4                   = 256 + 4;
    private static final int D_1_8                   = 256 + 8;
    private static final int D_1_10                  = 256 + 10;
    private static final int D_1_24                  = 256 + 24;
    private static final int D_1_28                  = 256 + 28;
    private static final int D_1_36                  = 256 + 36;
    private static final int D_1_40                  = 256 + 40;
    private static final int D_1_44                  = 256 + 44;
    private static final int D_1_72                  = 256 + 72;
    private static final int D_1_74                  = 256 + 74;
    private static final int D_1_79                  = 256 + 79;
    private static final int D_1_88                  = 256 + 88;
    private static final int D_1_90                  = 256 + 90;
    private static final int D_1_92                  = 256 + 92;
    private static final int D_1_96                  = 256 + 96;
    private static final int D_1_98                  = 256 + 98;
    private static final int D_1_106                 = 256 + 106;
    private static final int D_1_108                 = 256 + 108;
    private static final int D_1_112                 = 256 + 112;
    private static final int D_1_128                 = 256 + 144;
    private static final int D_1_152                 = 256 + 12;
    private static final int D_1_160                 = 256 + 160;
    private static final int D_1_176                 = 256 + 176;
    private static final int D_1_178                 = 256 + 178;
    private static final int D_1_218                 = 256 + 218;
    private static final int D_1_232                 = 256 + 232;
    private static final int D_1_236                 = 256 + 236;
    private static final int D_1_240                 = 256 + 240;
    private static final int D_1_244                 = 256 + 244;
    private static final int D_1_250                 = 256 + 250;
    private static final int D_1_252                 = 256 + 252;
    private static final int D_2_12                  = 512 + 12;
    private static final int D_2_96                  = 512 + 96;
    private static final int D_2_108                 = 512 + 108;
    private static final int D_2_208                 = 512 + 208;
    private static final int D_2_224                 = 512 + 224;
    private static final int D_2_236                 = 512 + 236;
    private static final int D_2_244                 = 512 + 244;
    private static final int D_2_248                 = 512 + 248;
    private static final int D_2_252                 = 512 + 252;

    private static final int CPASS_BIAS_X            = 35 * 16 + 4;
    private static final int CPASS_BIAS_Y            = 35 * 16 + 8;
    private static final int CPASS_BIAS_Z            = 35 * 16 + 12;
    private static final int CPASS_MTX_00            = 36 * 16;
    private static final int CPASS_MTX_01            = 36 * 16 + 4;
    private static final int CPASS_MTX_02            = 36 * 16 + 8;
    private static final int CPASS_MTX_10            = 36 * 16 + 12;
    private static final int CPASS_MTX_11            = 37 * 16;
    private static final int CPASS_MTX_12            = 37 * 16 + 4;
    private static final int CPASS_MTX_20            = 37 * 16 + 8;
    private static final int CPASS_MTX_21            = 37 * 16 + 12;
    private static final int CPASS_MTX_22            = 43 * 16 + 12;
    private static final int D_EXT_GYRO_BIAS_X       = 61 * 16;
    private static final int D_EXT_GYRO_BIAS_Y       = 61 * 16 + 4;
    private static final int D_EXT_GYRO_BIAS_Z       = 61 * 16 + 8;
    private static final int D_ACT0                  = 40 * 16;
    private static final int D_ACSX                  = 40 * 16 + 4;
    private static final int D_ACSY                  = 40 * 16 + 8;
    private static final int D_ACSZ                  = 40 * 16 + 12;

    private static final int FLICK_MSG               = 45 * 16 + 4;
    private static final int FLICK_COUNTER           = 45 * 16 + 8;
    private static final int FLICK_LOWER             = 45 * 16 + 12;
    private static final int FLICK_UPPER             = 46 * 16 + 12;

    private static final int D_AUTH_OUT              = 992;
    private static final int D_AUTH_IN               = 996;
    private static final int D_AUTH_A                = 1000;
    private static final int D_AUTH_B                = 1004;

    private static final int D_PEDSTD_BP_B           = 768 + 0x1C;
    private static final int D_PEDSTD_HP_A           = 768 + 0x78;
    private static final int D_PEDSTD_HP_B           = 768 + 0x7C;
    private static final int D_PEDSTD_BP_A4          = 768 + 0x40;
    private static final int D_PEDSTD_BP_A3          = 768 + 0x44;
    private static final int D_PEDSTD_BP_A2          = 768 + 0x48;
    private static final int D_PEDSTD_BP_A1          = 768 + 0x4C;
    private static final int D_PEDSTD_INT_THRSH      = 768 + 0x68;
    private static final int D_PEDSTD_CLIP           = 768 + 0x6C;
    private static final int D_PEDSTD_SB             = 768 + 0x28;
    private static final int D_PEDSTD_SB_TIME        = 768 + 0x2C;
    private static final int D_PEDSTD_PEAKTHRSH      = 768 + 0x98;
    private static final int D_PEDSTD_TIML           = 768 + 0x2A;
    private static final int D_PEDSTD_TIMH           = 768 + 0x2E;
    private static final int D_PEDSTD_PEAK           = 768 + 0X94;
    private static final int D_PEDSTD_STEPCTR        = 768 + 0x60;
    private static final int D_PEDSTD_TIMECTR        = 964;
    private static final int D_PEDSTD_DECI           = 768 + 0xA0;

    private static final int D_HOST_NO_MOT           = 976;
    private static final int D_ACCEL_BIAS            = 660;

    private static final int D_ORIENT_GAP            = 76;

    private static final int D_TILT0_H               = 48;
    private static final int D_TILT0_L               = 50;
    private static final int D_TILT1_H               = 52;
    private static final int D_TILT1_L               = 54;
    private static final int D_TILT2_H               = 56;
    private static final int D_TILT2_L               = 58;
    private static final int D_TILT3_H               = 60;
    private static final int D_TILT3_L               = 62;

    private static final int DMP_CODE_SIZE           = 3062;

    // first define as short[] and then cast to byte[]
    private static final short[] dmp_memory_short = {
        /* bank # 0 */
        0x00, 0x00, 0x70, 0x00, 0x00, 0x00, 0x00, 0x24, 0x00, 0x00, 0x00, 0x02, 0x00, 0x03, 0x00, 0x00,
        0x00, 0x65, 0x00, 0x54, 0xff, 0xef, 0x00, 0x00, 0xfa, 0x80, 0x00, 0x0b, 0x12, 0x82, 0x00, 0x01,
        0x03, 0x0c, 0x30, 0xc3, 0x0e, 0x8c, 0x8c, 0xe9, 0x14, 0xd5, 0x40, 0x02, 0x13, 0x71, 0x0f, 0x8e,
        0x38, 0x83, 0xf8, 0x83, 0x30, 0x00, 0xf8, 0x83, 0x25, 0x8e, 0xf8, 0x83, 0x30, 0x00, 0xf8, 0x83,
        0xff, 0xff, 0xff, 0xff, 0x0f, 0xfe, 0xa9, 0xd6, 0x24, 0x00, 0x04, 0x00, 0x1a, 0x82, 0x79, 0xa1,
        0x00, 0x00, 0x00, 0x3c, 0xff, 0xff, 0x00, 0x00, 0x00, 0x10, 0x00, 0x00, 0x38, 0x83, 0x6f, 0xa2,
        0x00, 0x3e, 0x03, 0x30, 0x40, 0x00, 0x00, 0x00, 0x02, 0xca, 0xe3, 0x09, 0x3e, 0x80, 0x00, 0x00,
        0x20, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x40, 0x00, 0x00, 0x00, 0x60, 0x00, 0x00, 0x00,
        0x00, 0x0c, 0x00, 0x00, 0x00, 0x0c, 0x18, 0x6e, 0x00, 0x00, 0x06, 0x92, 0x0a, 0x16, 0xc0, 0xdf,
        0xff, 0xff, 0x02, 0x56, 0xfd, 0x8c, 0xd3, 0x77, 0xff, 0xe1, 0xc4, 0x96, 0xe0, 0xc5, 0xbe, 0xaa,
        0x00, 0x00, 0x00, 0x00, 0xff, 0xff, 0x0b, 0x2b, 0x00, 0x00, 0x16, 0x57, 0x00, 0x00, 0x03, 0x59,
        0x40, 0x00, 0x00, 0x00, 0x00, 0x00, 0x1d, 0xfa, 0x00, 0x02, 0x6c, 0x1d, 0x00, 0x00, 0x00, 0x00,
        0x3f, 0xff, 0xdf, 0xeb, 0x00, 0x3e, 0xb3, 0xb6, 0x00, 0x0d, 0x22, 0x78, 0x00, 0x00, 0x2f, 0x3c,
        0x00, 0x00, 0x00, 0x00, 0x00, 0x19, 0x42, 0xb5, 0x00, 0x00, 0x39, 0xa2, 0x00, 0x00, 0xb3, 0x65,
        0xd9, 0x0e, 0x9f, 0xc9, 0x1d, 0xcf, 0x4c, 0x34, 0x30, 0x00, 0x00, 0x00, 0x50, 0x00, 0x00, 0x00,
        0x3b, 0xb6, 0x7a, 0xe8, 0x00, 0x64, 0x00, 0x00, 0x00, 0xc8, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
        /* bank # 1 */
        0x10, 0x00, 0x00, 0x00, 0x10, 0x00, 0xfa, 0x92, 0x10, 0x00, 0x22, 0x5e, 0x00, 0x0d, 0x22, 0x9f,
        0x00, 0x01, 0x00, 0x00, 0x00, 0x32, 0x00, 0x00, 0xff, 0x46, 0x00, 0x00, 0x63, 0xd4, 0x00, 0x00,
        0x10, 0x00, 0x00, 0x00, 0x04, 0xd6, 0x00, 0x00, 0x04, 0xcc, 0x00, 0x00, 0x04, 0xcc, 0x00, 0x00,
        0x00, 0x00, 0x10, 0x72, 0x00, 0x00, 0x00, 0x40, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
        0x00, 0x06, 0x00, 0x02, 0x00, 0x05, 0x00, 0x07, 0x00, 0x00, 0x00, 0x00, 0x00, 0x64, 0x00, 0x00,
        0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x05, 0x00, 0x05, 0x00, 0x64, 0x00, 0x20, 0x00, 0x00,
        0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x40, 0x00, 0x00, 0x00, 0x03, 0x00,
        0x00, 0x00, 0x00, 0x32, 0xf8, 0x98, 0x00, 0x00, 0xff, 0x65, 0x00, 0x00, 0x83, 0x0f, 0x00, 0x00,
        0xff, 0x9b, 0xfc, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
        0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
        0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x10, 0x00,
        0x40, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x06, 0x00, 0x00, 0xb2, 0x6a, 0x00, 0x02, 0x00, 0x00,
        0x00, 0x01, 0xfb, 0x83, 0x00, 0x68, 0x00, 0x00, 0x00, 0xd9, 0xfc, 0x00, 0x7c, 0xf1, 0xff, 0x83,
        0x00, 0x00, 0x00, 0x00, 0x00, 0x65, 0x00, 0x00, 0x00, 0x64, 0x03, 0xe8, 0x00, 0x64, 0x00, 0x28,
        0x00, 0x00, 0x00, 0x25, 0x00, 0x00, 0x00, 0x00, 0x16, 0xa0, 0x00, 0x00, 0x00, 0x00, 0x10, 0x00,
        0x00, 0x00, 0x10, 0x00, 0x00, 0x2f, 0x00, 0x00, 0x00, 0x00, 0x01, 0xf4, 0x00, 0x00, 0x10, 0x00,
        /* bank # 2 */
        0x00, 0x28, 0x00, 0x00, 0xff, 0xff, 0x45, 0x81, 0xff, 0xff, 0xfa, 0x72, 0x00, 0x00, 0x00, 0x00,
        0x00, 0x00, 0x00, 0x00, 0x00, 0x44, 0x00, 0x05, 0x00, 0x05, 0xba, 0xc6, 0x00, 0x47, 0x78, 0xa2,
        0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x06, 0x00, 0x00, 0x00, 0x00, 0x14,
        0x00, 0x00, 0x25, 0x4d, 0x00, 0x2f, 0x70, 0x6d, 0x00, 0x00, 0x05, 0xae, 0x00, 0x0c, 0x02, 0xd0,
        0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
        0x00, 0x1b, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
        0x00, 0x64, 0x00, 0x00, 0x00, 0x08, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
        0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
        0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
        0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
        0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
        0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
        0x00, 0x1b, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x0e, 0x00, 0x0e,
        0x00, 0x00, 0x0a, 0xc7, 0x00, 0x04, 0x00, 0x00, 0x00, 0x00, 0x00, 0x32, 0xff, 0xff, 0xff, 0x9c,
        0x00, 0x00, 0x0b, 0x2b, 0x00, 0x00, 0x00, 0x02, 0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00, 0x64,
        0xff, 0xe5, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
        /* bank # 3 */
        0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
        0x00, 0x01, 0x80, 0x00, 0x00, 0x01, 0x80, 0x00, 0x00, 0x01, 0x80, 0x00, 0x00, 0x24, 0x26, 0xd3,
        0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x06, 0x00, 0x10, 0x00, 0x96, 0x00, 0x3c,
        0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
        0x0c, 0x0a, 0x4e, 0x68, 0xcd, 0xcf, 0x77, 0x09, 0x50, 0x16, 0x67, 0x59, 0xc6, 0x19, 0xce, 0x82,
        0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
        0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x17, 0xd7, 0x84, 0x00, 0x03, 0x00, 0x00, 0x00,
        0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0xc7, 0x93, 0x8f, 0x9d, 0x1e, 0x1b, 0x1c, 0x19,
        0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
        0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x02, 0x03, 0x18, 0x85, 0x00, 0x00, 0x40, 0x00,
        0x00, 0x00, 0x00, 0x03, 0x00, 0x00, 0x00, 0x03, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
        0x40, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
        0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
        0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
        0x00, 0x00, 0x00, 0x00, 0x67, 0x7d, 0xdf, 0x7e, 0x72, 0x90, 0x2e, 0x55, 0x4c, 0xf6, 0xe6, 0x88,
        0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,

        /* bank # 4 */
        0xd8, 0xdc, 0xb4, 0xb8, 0xb0, 0xd8, 0xb9, 0xab, 0xf3, 0xf8, 0xfa, 0xb3, 0xb7, 0xbb, 0x8e, 0x9e,
        0xae, 0xf1, 0x32, 0xf5, 0x1b, 0xf1, 0xb4, 0xb8, 0xb0, 0x80, 0x97, 0xf1, 0xa9, 0xdf, 0xdf, 0xdf,
        0xaa, 0xdf, 0xdf, 0xdf, 0xf2, 0xaa, 0xc5, 0xcd, 0xc7, 0xa9, 0x0c, 0xc9, 0x2c, 0x97, 0xf1, 0xa9,
        0x89, 0x26, 0x46, 0x66, 0xb2, 0x89, 0x99, 0xa9, 0x2d, 0x55, 0x7d, 0xb0, 0xb0, 0x8a, 0xa8, 0x96,
        0x36, 0x56, 0x76, 0xf1, 0xba, 0xa3, 0xb4, 0xb2, 0x80, 0xc0, 0xb8, 0xa8, 0x97, 0x11, 0xb2, 0x83,
        0x98, 0xba, 0xa3, 0xf0, 0x24, 0x08, 0x44, 0x10, 0x64, 0x18, 0xb2, 0xb9, 0xb4, 0x98, 0x83, 0xf1,
        0xa3, 0x29, 0x55, 0x7d, 0xba, 0xb5, 0xb1, 0xa3, 0x83, 0x93, 0xf0, 0x00, 0x28, 0x50, 0xf5, 0xb2,
        0xb6, 0xaa, 0x83, 0x93, 0x28, 0x54, 0x7c, 0xf1, 0xb9, 0xa3, 0x82, 0x93, 0x61, 0xba, 0xa2, 0xda,
        0xde, 0xdf, 0xdb, 0x81, 0x9a, 0xb9, 0xae, 0xf5, 0x60, 0x68, 0x70, 0xf1, 0xda, 0xba, 0xa2, 0xdf,
        0xd9, 0xba, 0xa2, 0xfa, 0xb9, 0xa3, 0x82, 0x92, 0xdb, 0x31, 0xba, 0xa2, 0xd9, 0xba, 0xa2, 0xf8,
        0xdf, 0x85, 0xa4, 0xd0, 0xc1, 0xbb, 0xad, 0x83, 0xc2, 0xc5, 0xc7, 0xb8, 0xa2, 0xdf, 0xdf, 0xdf,
        0xba, 0xa0, 0xdf, 0xdf, 0xdf, 0xd8, 0xd8, 0xf1, 0xb8, 0xaa, 0xb3, 0x8d, 0xb4, 0x98, 0x0d, 0x35,
        0x5d, 0xb2, 0xb6, 0xba, 0xaf, 0x8c, 0x96, 0x19, 0x8f, 0x9f, 0xa7, 0x0e, 0x16, 0x1e, 0xb4, 0x9a,
        0xb8, 0xaa, 0x87, 0x2c, 0x54, 0x7c, 0xba, 0xa4, 0xb0, 0x8a, 0xb6, 0x91, 0x32, 0x56, 0x76, 0xb2,
        0x84, 0x94, 0xa4, 0xc8, 0x08, 0xcd, 0xd8, 0xb8, 0xb4, 0xb0, 0xf1, 0x99, 0x82, 0xa8, 0x2d, 0x55,
        0x7d, 0x98, 0xa8, 0x0e, 0x16, 0x1e, 0xa2, 0x2c, 0x54, 0x7c, 0x92, 0xa4, 0xf0, 0x2c, 0x50, 0x78,
        /* bank # 5 */
        0xf1, 0x84, 0xa8, 0x98, 0xc4, 0xcd, 0xfc, 0xd8, 0x0d, 0xdb, 0xa8, 0xfc, 0x2d, 0xf3, 0xd9, 0xba,
        0xa6, 0xf8, 0xda, 0xba, 0xa6, 0xde, 0xd8, 0xba, 0xb2, 0xb6, 0x86, 0x96, 0xa6, 0xd0, 0xf3, 0xc8,
        0x41, 0xda, 0xa6, 0xc8, 0xf8, 0xd8, 0xb0, 0xb4, 0xb8, 0x82, 0xa8, 0x92, 0xf5, 0x2c, 0x54, 0x88,
        0x98, 0xf1, 0x35, 0xd9, 0xf4, 0x18, 0xd8, 0xf1, 0xa2, 0xd0, 0xf8, 0xf9, 0xa8, 0x84, 0xd9, 0xc7,
        0xdf, 0xf8, 0xf8, 0x83, 0xc5, 0xda, 0xdf, 0x69, 0xdf, 0x83, 0xc1, 0xd8, 0xf4, 0x01, 0x14, 0xf1,
        0xa8, 0x82, 0x4e, 0xa8, 0x84, 0xf3, 0x11, 0xd1, 0x82, 0xf5, 0xd9, 0x92, 0x28, 0x97, 0x88, 0xf1,
        0x09, 0xf4, 0x1c, 0x1c, 0xd8, 0x84, 0xa8, 0xf3, 0xc0, 0xf9, 0xd1, 0xd9, 0x97, 0x82, 0xf1, 0x29,
        0xf4, 0x0d, 0xd8, 0xf3, 0xf9, 0xf9, 0xd1, 0xd9, 0x82, 0xf4, 0xc2, 0x03, 0xd8, 0xde, 0xdf, 0x1a,
        0xd8, 0xf1, 0xa2, 0xfa, 0xf9, 0xa8, 0x84, 0x98, 0xd9, 0xc7, 0xdf, 0xf8, 0xf8, 0xf8, 0x83, 0xc7,
        0xda, 0xdf, 0x69, 0xdf, 0xf8, 0x83, 0xc3, 0xd8, 0xf4, 0x01, 0x14, 0xf1, 0x98, 0xa8, 0x82, 0x2e,
        0xa8, 0x84, 0xf3, 0x11, 0xd1, 0x82, 0xf5, 0xd9, 0x92, 0x50, 0x97, 0x88, 0xf1, 0x09, 0xf4, 0x1c,
        0xd8, 0x84, 0xa8, 0xf3, 0xc0, 0xf8, 0xf9, 0xd1, 0xd9, 0x97, 0x82, 0xf1, 0x49, 0xf4, 0x0d, 0xd8,
        0xf3, 0xf9, 0xf9, 0xd1, 0xd9, 0x82, 0xf4, 0xc4, 0x03, 0xd8, 0xde, 0xdf, 0xd8, 0xf1, 0xad, 0x88,
        0x98, 0xcc, 0xa8, 0x09, 0xf9, 0xd9, 0x82, 0x92, 0xa8, 0xf5, 0x7c, 0xf1, 0x88, 0x3a, 0xcf, 0x94,
        0x4a, 0x6e, 0x98, 0xdb, 0x69, 0x31, 0xda, 0xad, 0xf2, 0xde, 0xf9, 0xd8, 0x87, 0x95, 0xa8, 0xf2,
        0x21, 0xd1, 0xda, 0xa5, 0xf9, 0xf4, 0x17, 0xd9, 0xf1, 0xae, 0x8e, 0xd0, 0xc0, 0xc3, 0xae, 0x82,
        /* bank # 6 */
        0xc6, 0x84, 0xc3, 0xa8, 0x85, 0x95, 0xc8, 0xa5, 0x88, 0xf2, 0xc0, 0xf1, 0xf4, 0x01, 0x0e, 0xf1,
        0x8e, 0x9e, 0xa8, 0xc6, 0x3e, 0x56, 0xf5, 0x54, 0xf1, 0x88, 0x72, 0xf4, 0x01, 0x15, 0xf1, 0x98,
        0x45, 0x85, 0x6e, 0xf5, 0x8e, 0x9e, 0x04, 0x88, 0xf1, 0x42, 0x98, 0x5a, 0x8e, 0x9e, 0x06, 0x88,
        0x69, 0xf4, 0x01, 0x1c, 0xf1, 0x98, 0x1e, 0x11, 0x08, 0xd0, 0xf5, 0x04, 0xf1, 0x1e, 0x97, 0x02,
        0x02, 0x98, 0x36, 0x25, 0xdb, 0xf9, 0xd9, 0x85, 0xa5, 0xf3, 0xc1, 0xda, 0x85, 0xa5, 0xf3, 0xdf,
        0xd8, 0x85, 0x95, 0xa8, 0xf3, 0x09, 0xda, 0xa5, 0xfa, 0xd8, 0x82, 0x92, 0xa8, 0xf5, 0x78, 0xf1,
        0x88, 0x1a, 0x84, 0x9f, 0x26, 0x88, 0x98, 0x21, 0xda, 0xf4, 0x1d, 0xf3, 0xd8, 0x87, 0x9f, 0x39,
        0xd1, 0xaf, 0xd9, 0xdf, 0xdf, 0xfb, 0xf9, 0xf4, 0x0c, 0xf3, 0xd8, 0xfa, 0xd0, 0xf8, 0xda, 0xf9,
        0xf9, 0xd0, 0xdf, 0xd9, 0xf9, 0xd8, 0xf4, 0x0b, 0xd8, 0xf3, 0x87, 0x9f, 0x39, 0xd1, 0xaf, 0xd9,
        0xdf, 0xdf, 0xf4, 0x1d, 0xf3, 0xd8, 0xfa, 0xfc, 0xa8, 0x69, 0xf9, 0xf9, 0xaf, 0xd0, 0xda, 0xde,
        0xfa, 0xd9, 0xf8, 0x8f, 0x9f, 0xa8, 0xf1, 0xcc, 0xf3, 0x98, 0xdb, 0x45, 0xd9, 0xaf, 0xdf, 0xd0,
        0xf8, 0xd8, 0xf1, 0x8f, 0x9f, 0xa8, 0xca, 0xf3, 0x88, 0x09, 0xda, 0xaf, 0x8f, 0xcb, 0xf8, 0xd8,
        0xf2, 0xad, 0x97, 0x8d, 0x0c, 0xd9, 0xa5, 0xdf, 0xf9, 0xba, 0xa6, 0xf3, 0xfa, 0xf4, 0x12, 0xf2,
        0xd8, 0x95, 0x0d, 0xd1, 0xd9, 0xba, 0xa6, 0xf3, 0xfa, 0xda, 0xa5, 0xf2, 0xc1, 0xba, 0xa6, 0xf3,
        0xdf, 0xd8, 0xf1, 0xba, 0xb2, 0xb6, 0x86, 0x96, 0xa6, 0xd0, 0xca, 0xf3, 0x49, 0xda, 0xa6, 0xcb,
        0xf8, 0xd8, 0xb0, 0xb4, 0xb8, 0xd8, 0xad, 0x84, 0xf2, 0xc0, 0xdf, 0xf1, 0x8f, 0xcb, 0xc3, 0xa8,
        /* bank # 7 */
        0xb2, 0xb6, 0x86, 0x96, 0xc8, 0xc1, 0xcb, 0xc3, 0xf3, 0xb0, 0xb4, 0x88, 0x98, 0xa8, 0x21, 0xdb,
        0x71, 0x8d, 0x9d, 0x71, 0x85, 0x95, 0x21, 0xd9, 0xad, 0xf2, 0xfa, 0xd8, 0x85, 0x97, 0xa8, 0x28,
        0xd9, 0xf4, 0x08, 0xd8, 0xf2, 0x8d, 0x29, 0xda, 0xf4, 0x05, 0xd9, 0xf2, 0x85, 0xa4, 0xc2, 0xf2,
        0xd8, 0xa8, 0x8d, 0x94, 0x01, 0xd1, 0xd9, 0xf4, 0x11, 0xf2, 0xd8, 0x87, 0x21, 0xd8, 0xf4, 0x0a,
        0xd8, 0xf2, 0x84, 0x98, 0xa8, 0xc8, 0x01, 0xd1, 0xd9, 0xf4, 0x11, 0xd8, 0xf3, 0xa4, 0xc8, 0xbb,
        0xaf, 0xd0, 0xf2, 0xde, 0xf8, 0xf8, 0xf8, 0xf8, 0xf8, 0xf8, 0xf8, 0xf8, 0xd8, 0xf1, 0xb8, 0xf6,
        0xb5, 0xb9, 0xb0, 0x8a, 0x95, 0xa3, 0xde, 0x3c, 0xa3, 0xd9, 0xf8, 0xd8, 0x5c, 0xa3, 0xd9, 0xf8,
        0xd8, 0x7c, 0xa3, 0xd9, 0xf8, 0xd8, 0xf8, 0xf9, 0xd1, 0xa5, 0xd9, 0xdf, 0xda, 0xfa, 0xd8, 0xb1,
        0x85, 0x30, 0xf7, 0xd9, 0xde, 0xd8, 0xf8, 0x30, 0xad, 0xda, 0xde, 0xd8, 0xf2, 0xb4, 0x8c, 0x99,
        0xa3, 0x2d, 0x55, 0x7d, 0xa0, 0x83, 0xdf, 0xdf, 0xdf, 0xb5, 0x91, 0xa0, 0xf6, 0x29, 0xd9, 0xfb,
        0xd8, 0xa0, 0xfc, 0x29, 0xd9, 0xfa, 0xd8, 0xa0, 0xd0, 0x51, 0xd9, 0xf8, 0xd8, 0xfc, 0x51, 0xd9,
        0xf9, 0xd8, 0x79, 0xd9, 0xfb, 0xd8, 0xa0, 0xd0, 0xfc, 0x79, 0xd9, 0xfa, 0xd8, 0xa1, 0xf9, 0xf9,
        0xf9, 0xf9, 0xf9, 0xa0, 0xda, 0xdf, 0xdf, 0xdf, 0xd8, 0xa1, 0xf8, 0xf8, 0xf8, 0xf8, 0xf8, 0xac,
        0xde, 0xf8, 0xad, 0xde, 0x83, 0x93, 0xac, 0x2c, 0x54, 0x7c, 0xf1, 0xa8, 0xdf, 0xdf, 0xdf, 0xf6,
        0x9d, 0x2c, 0xda, 0xa0, 0xdf, 0xd9, 0xfa, 0xdb, 0x2d, 0xf8, 0xd8, 0xa8, 0x50, 0xda, 0xa0, 0xd0,
        0xde, 0xd9, 0xd0, 0xf8, 0xf8, 0xf8, 0xdb, 0x55, 0xf8, 0xd8, 0xa8, 0x78, 0xda, 0xa0, 0xd0, 0xdf,
        /* bank # 8 */
        0xd9, 0xd0, 0xfa, 0xf8, 0xf8, 0xf8, 0xf8, 0xdb, 0x7d, 0xf8, 0xd8, 0x9c, 0xa8, 0x8c, 0xf5, 0x30,
        0xdb, 0x38, 0xd9, 0xd0, 0xde, 0xdf, 0xa0, 0xd0, 0xde, 0xdf, 0xd8, 0xa8, 0x48, 0xdb, 0x58, 0xd9,
        0xdf, 0xd0, 0xde, 0xa0, 0xdf, 0xd0, 0xde, 0xd8, 0xa8, 0x68, 0xdb, 0x70, 0xd9, 0xdf, 0xdf, 0xa0,
        0xdf, 0xdf, 0xd8, 0xf1, 0xa8, 0x88, 0x90, 0x2c, 0x54, 0x7c, 0x98, 0xa8, 0xd0, 0x5c, 0x38, 0xd1,
        0xda, 0xf2, 0xae, 0x8c, 0xdf, 0xf9, 0xd8, 0xb0, 0x87, 0xa8, 0xc1, 0xc1, 0xb1, 0x88, 0xa8, 0xc6,
        0xf9, 0xf9, 0xda, 0x36, 0xd8, 0xa8, 0xf9, 0xda, 0x36, 0xd8, 0xa8, 0xf9, 0xda, 0x36, 0xd8, 0xa8,
        0xf9, 0xda, 0x36, 0xd8, 0xa8, 0xf9, 0xda, 0x36, 0xd8, 0xf7, 0x8d, 0x9d, 0xad, 0xf8, 0x18, 0xda,
        0xf2, 0xae, 0xdf, 0xd8, 0xf7, 0xad, 0xfa, 0x30, 0xd9, 0xa4, 0xde, 0xf9, 0xd8, 0xf2, 0xae, 0xde,
        0xfa, 0xf9, 0x83, 0xa7, 0xd9, 0xc3, 0xc5, 0xc7, 0xf1, 0x88, 0x9b, 0xa7, 0x7a, 0xad, 0xf7, 0xde,
        0xdf, 0xa4, 0xf8, 0x84, 0x94, 0x08, 0xa7, 0x97, 0xf3, 0x00, 0xae, 0xf2, 0x98, 0x19, 0xa4, 0x88,
        0xc6, 0xa3, 0x94, 0x88, 0xf6, 0x32, 0xdf, 0xf2, 0x83, 0x93, 0xdb, 0x09, 0xd9, 0xf2, 0xaa, 0xdf,
        0xd8, 0xd8, 0xae, 0xf8, 0xf9, 0xd1, 0xda, 0xf3, 0xa4, 0xde, 0xa7, 0xf1, 0x88, 0x9b, 0x7a, 0xd8,
        0xf3, 0x84, 0x94, 0xae, 0x19, 0xf9, 0xda, 0xaa, 0xf1, 0xdf, 0xd8, 0xa8, 0x81, 0xc0, 0xc3, 0xc5,
        0xc7, 0xa3, 0x92, 0x83, 0xf6, 0x28, 0xad, 0xde, 0xd9, 0xf8, 0xd8, 0xa3, 0x50, 0xad, 0xd9, 0xf8,
        0xd8, 0xa3, 0x78, 0xad, 0xd9, 0xf8, 0xd8, 0xf8, 0xf9, 0xd1, 0xa1, 0xda, 0xde, 0xc3, 0xc5, 0xc7,
        0xd8, 0xa1, 0x81, 0x94, 0xf8, 0x18, 0xf2, 0xb0, 0x89, 0xac, 0xc3, 0xc5, 0xc7, 0xf1, 0xd8, 0xb8,
        /* bank # 9 */
        0xb4, 0xb0, 0x97, 0x86, 0xa8, 0x31, 0x9b, 0x06, 0x99, 0x07, 0xab, 0x97, 0x28, 0x88, 0x9b, 0xf0,
        0x0c, 0x20, 0x14, 0x40, 0xb0, 0xb4, 0xb8, 0xf0, 0xa8, 0x8a, 0x9a, 0x28, 0x50, 0x78, 0xb7, 0x9b,
        0xa8, 0x29, 0x51, 0x79, 0x24, 0x70, 0x59, 0x44, 0x69, 0x38, 0x64, 0x48, 0x31, 0xf1, 0xbb, 0xab,
        0x88, 0x00, 0x2c, 0x54, 0x7c, 0xf0, 0xb3, 0x8b, 0xb8, 0xa8, 0x04, 0x28, 0x50, 0x78, 0xf1, 0xb0,
        0x88, 0xb4, 0x97, 0x26, 0xa8, 0x59, 0x98, 0xbb, 0xab, 0xb3, 0x8b, 0x02, 0x26, 0x46, 0x66, 0xb0,
        0xb8, 0xf0, 0x8a, 0x9c, 0xa8, 0x29, 0x51, 0x79, 0x8b, 0x29, 0x51, 0x79, 0x8a, 0x24, 0x70, 0x59,
        0x8b, 0x20, 0x58, 0x71, 0x8a, 0x44, 0x69, 0x38, 0x8b, 0x39, 0x40, 0x68, 0x8a, 0x64, 0x48, 0x31,
        0x8b, 0x30, 0x49, 0x60, 0x88, 0xf1, 0xac, 0x00, 0x2c, 0x54, 0x7c, 0xf0, 0x8c, 0xa8, 0x04, 0x28,
        0x50, 0x78, 0xf1, 0x88, 0x97, 0x26, 0xa8, 0x59, 0x98, 0xac, 0x8c, 0x02, 0x26, 0x46, 0x66, 0xf0,
        0x89, 0x9c, 0xa8, 0x29, 0x51, 0x79, 0x24, 0x70, 0x59, 0x44, 0x69, 0x38, 0x64, 0x48, 0x31, 0xa9,
        0x88, 0x09, 0x20, 0x59, 0x70, 0xab, 0x11, 0x38, 0x40, 0x69, 0xa8, 0x19, 0x31, 0x48, 0x60, 0x8c,
        0xa8, 0x3c, 0x41, 0x5c, 0x20, 0x7c, 0x00, 0xf1, 0x87, 0x98, 0x19, 0x86, 0xa8, 0x6e, 0x76, 0x7e,
        0xa9, 0x99, 0x88, 0x2d, 0x55, 0x7d, 0xd8, 0xb1, 0xb5, 0xb9, 0xa3, 0xdf, 0xdf, 0xdf, 0xae, 0xd0,
        0xdf, 0xaa, 0xd0, 0xde, 0xf2, 0xab, 0xf8, 0xf9, 0xd9, 0xb0, 0x87, 0xc4, 0xaa, 0xf1, 0xdf, 0xdf,
        0xbb, 0xaf, 0xdf, 0xdf, 0xb9, 0xd8, 0xb1, 0xf1, 0xa3, 0x97, 0x8e, 0x60, 0xdf, 0xb0, 0x84, 0xf2,
        0xc8, 0xf8, 0xf9, 0xd9, 0xde, 0xd8, 0x93, 0x85, 0xf1, 0x4a, 0xb1, 0x83, 0xa3, 0x08, 0xb5, 0x83,
        /* bank # 10 */
        0x9a, 0x08, 0x10, 0xb7, 0x9f, 0x10, 0xd8, 0xf1, 0xb0, 0xba, 0xae, 0xb0, 0x8a, 0xc2, 0xb2, 0xb6,
        0x8e, 0x9e, 0xf1, 0xfb, 0xd9, 0xf4, 0x1d, 0xd8, 0xf9, 0xd9, 0x0c, 0xf1, 0xd8, 0xf8, 0xf8, 0xad,
        0x61, 0xd9, 0xae, 0xfb, 0xd8, 0xf4, 0x0c, 0xf1, 0xd8, 0xf8, 0xf8, 0xad, 0x19, 0xd9, 0xae, 0xfb,
        0xdf, 0xd8, 0xf4, 0x16, 0xf1, 0xd8, 0xf8, 0xad, 0x8d, 0x61, 0xd9, 0xf4, 0xf4, 0xac, 0xf5, 0x9c,
        0x9c, 0x8d, 0xdf, 0x2b, 0xba, 0xb6, 0xae, 0xfa, 0xf8, 0xf4, 0x0b, 0xd8, 0xf1, 0xae, 0xd0, 0xf8,
        0xad, 0x51, 0xda, 0xae, 0xfa, 0xf8, 0xf1, 0xd8, 0xb9, 0xb1, 0xb6, 0xa3, 0x83, 0x9c, 0x08, 0xb9,
        0xb1, 0x83, 0x9a, 0xb5, 0xaa, 0xc0, 0xfd, 0x30, 0x83, 0xb7, 0x9f, 0x10, 0xb5, 0x8b, 0x93, 0xf2,
        0x02, 0x02, 0xd1, 0xab, 0xda, 0xde, 0xd8, 0xf1, 0xb0, 0x80, 0xba, 0xab, 0xc0, 0xc3, 0xb2, 0x84,
        0xc1, 0xc3, 0xd8, 0xb1, 0xb9, 0xf3, 0x8b, 0xa3, 0x91, 0xb6, 0x09, 0xb4, 0xd9, 0xab, 0xde, 0xb0,
        0x87, 0x9c, 0xb9, 0xa3, 0xdd, 0xf1, 0xb3, 0x8b, 0x8b, 0x8b, 0x8b, 0x8b, 0xb0, 0x87, 0xa3, 0xa3,
        0xa3, 0xa3, 0xb2, 0x8b, 0xb6, 0x9b, 0xf2, 0xa3, 0xa3, 0xa3, 0xa3, 0xa3, 0xa3, 0xa3, 0xa3, 0xa3,
        0xa3, 0xf1, 0xb0, 0x87, 0xb5, 0x9a, 0xa3, 0xf3, 0x9b, 0xa3, 0xa3, 0xdc, 0xba, 0xac, 0xdf, 0xb9,
        0xa3, 0xa3, 0xa3, 0xa3, 0xa3, 0xa3, 0xa3, 0xa3, 0xa3, 0xa3, 0xa3, 0xa3, 0xa3, 0xa3, 0xa3, 0xa3,
        0xd8, 0xd8, 0xd8, 0xbb, 0xb3, 0xb7, 0xf1, 0xaa, 0xf9, 0xda, 0xff, 0xd9, 0x80, 0x9a, 0xaa, 0x28,
        0xb4, 0x80, 0x98, 0xa7, 0x20, 0xb7, 0x97, 0x87, 0xa8, 0x66, 0x88, 0xf0, 0x79, 0x51, 0xf1, 0x90,
        0x2c, 0x87, 0x0c, 0xa7, 0x81, 0x97, 0x62, 0x93, 0xf0, 0x71, 0x71, 0x60, 0x85, 0x94, 0x01, 0x29,
        /* bank # 11 */
        0x51, 0x79, 0x90, 0xa5, 0xf1, 0x28, 0x4c, 0x6c, 0x87, 0x0c, 0x95, 0x18, 0x85, 0x78, 0xa3, 0x83,
        0x90, 0x28, 0x4c, 0x6c, 0x88, 0x6c, 0xd8, 0xf3, 0xa2, 0x82, 0x00, 0xf2, 0x10, 0xa8, 0x92, 0x19,
        0x80, 0xa2, 0xf2, 0xd9, 0x26, 0xd8, 0xf1, 0x88, 0xa8, 0x4d, 0xd9, 0x48, 0xd8, 0x96, 0xa8, 0x39,
        0x80, 0xd9, 0x3c, 0xd8, 0x95, 0x80, 0xa8, 0x39, 0xa6, 0x86, 0x98, 0xd9, 0x2c, 0xda, 0x87, 0xa7,
        0x2c, 0xd8, 0xa8, 0x89, 0x95, 0x19, 0xa9, 0x80, 0xd9, 0x38, 0xd8, 0xa8, 0x89, 0x39, 0xa9, 0x80,
        0xda, 0x3c, 0xd8, 0xa8, 0x2e, 0xa8, 0x39, 0x90, 0xd9, 0x0c, 0xd8, 0xa8, 0x95, 0x31, 0x98, 0xd9,
        0x0c, 0xd8, 0xa8, 0x09, 0xd9, 0xff, 0xd8, 0x01, 0xda, 0xff, 0xd8, 0x95, 0x39, 0xa9, 0xda, 0x26,
        0xff, 0xd8, 0x90, 0xa8, 0x0d, 0x89, 0x99, 0xa8, 0x10, 0x80, 0x98, 0x21, 0xda, 0x2e, 0xd8, 0x89,
        0x99, 0xa8, 0x31, 0x80, 0xda, 0x2e, 0xd8, 0xa8, 0x86, 0x96, 0x31, 0x80, 0xda, 0x2e, 0xd8, 0xa8,
        0x87, 0x31, 0x80, 0xda, 0x2e, 0xd8, 0xa8, 0x82, 0x92, 0xf3, 0x41, 0x80, 0xf1, 0xd9, 0x2e, 0xd8,
        0xa8, 0x82, 0xf3, 0x19, 0x80, 0xf1, 0xd9, 0x2e, 0xd8, 0x82, 0xac, 0xf3, 0xc0, 0xa2, 0x80, 0x22,
        0xf1, 0xa6, 0x2e, 0xa7, 0x2e, 0xa9, 0x22, 0x98, 0xa8, 0x29, 0xda, 0xac, 0xde, 0xff, 0xd8, 0xa2,
        0xf2, 0x2a, 0xf1, 0xa9, 0x2e, 0x82, 0x92, 0xa8, 0xf2, 0x31, 0x80, 0xa6, 0x96, 0xf1, 0xd9, 0x00,
        0xac, 0x8c, 0x9c, 0x0c, 0x30, 0xac, 0xde, 0xd0, 0xde, 0xff, 0xd8, 0x8c, 0x9c, 0xac, 0xd0, 0x10,
        0xac, 0xde, 0x80, 0x92, 0xa2, 0xf2, 0x4c, 0x82, 0xa8, 0xf1, 0xca, 0xf2, 0x35, 0xf1, 0x96, 0x88,
        0xa6, 0xd9, 0x00, 0xd8, 0xf1, 0xff
    };
    private static final byte[] dmp_memory = new byte[dmp_memory_short.length];
    static {
        for (int i = 0; i < dmp_memory.length; i++) {
            dmp_memory[i] = (byte) dmp_memory_short[i];
        }
    }

    private static final short sStartAddress = 0x0400;

    /* END OF SECTION COPIED FROM dmpDefaultMPU6050.c */

    private static final int INT_SRC_TAP             = 0x01;
    private static final int INT_SRC_ANDROID_ORIENT  = 0x08;

    private static final int DMP_FEATURE_SEND_ANY_GYRO   = DMP_FEATURE_SEND_RAW_GYRO | DMP_FEATURE_SEND_CAL_GYRO;

    private static final int MAX_PACKET_LENGTH   = 32;

    private static final int DMP_SAMPLE_RATE     = 200;
    private static final int GYRO_SF             = (int) (46850825L * 200 / DMP_SAMPLE_RATE);

    private static final int QUAT_ERROR_THRESH       = 1<<24;
    private static final int QUAT_MAG_SQ_NORMALIZED  = 1<<28;
    private static final int QUAT_MAG_SQ_MIN         = QUAT_MAG_SQ_NORMALIZED - QUAT_ERROR_THRESH;
    private static final int QUAT_MAG_SQ_MAX         = QUAT_MAG_SQ_NORMALIZED + QUAT_ERROR_THRESH;

    private static class dmp_s {
        int orient;
        int feature_mask;
        int fifo_rate;
        short packet_length;
    };
    private static dmp_s dmp = new dmp_s();
    static {
        dmp.orient = 0;
        dmp.feature_mask = 0;
        dmp.fifo_rate = 0;
        dmp.packet_length = 0;
    }

    private final InvMpu invMpu;

    public InvMpuDmpMotionDriver(InvMpu invMpu) {
        this.invMpu = invMpu;
    }

    /**
     *  @brief  Load the DMP with this image.
     *  @return 0 if successful.
     */
    boolean dmp_load_motion_driver_firmware()
    {
        return invMpu.mpu_load_firmware(DMP_CODE_SIZE, dmp_memory, sStartAddress, DMP_SAMPLE_RATE);
    }

    /**
     *  @brief      Push gyro and accel orientation to the DMP.
     *  The orientation is represented here as the output of
     *  @e inv_orientation_matrix_to_scalar.
     *  @param[in]  orient  Gyro and accel orientation in body frame.
     *  @return     0 if successful.
     */
    boolean dmp_set_orientation(int orient)
    {
        byte[] gyro_regs = new byte[3];
        byte[] accel_regs = new byte[3];
        byte[] gyro_axes = {(byte)DmpKey.DINA4C, (byte)DmpKey.DINACD, (byte)DmpKey.DINA6C};
        byte[] accel_axes = {(byte)DmpKey.DINA0C, (byte)DmpKey.DINAC9, (byte)DmpKey.DINA2C};
        byte[] gyro_sign = {(byte)DmpKey.DINA36, (byte)DmpKey.DINA56, (byte)DmpKey.DINA76};
        byte[] accel_sign = {(byte)DmpKey.DINA26, (byte)DmpKey.DINA46, (byte)DmpKey.DINA66};

        gyro_regs[0] = gyro_axes[orient & 3];
        gyro_regs[1] = gyro_axes[(orient >> 3) & 3];
        gyro_regs[2] = gyro_axes[(orient >> 6) & 3];
        accel_regs[0] = accel_axes[orient & 3];
        accel_regs[1] = accel_axes[(orient >> 3) & 3];
        accel_regs[2] = accel_axes[(orient >> 6) & 3];

        /* Chip-to-body, axes only. */
        if (!invMpu.mpu_write_mem((short)FCFG_1, gyro_regs, 0, 3))
            return false;
        if (!invMpu.mpu_write_mem((short)FCFG_2, accel_regs, 0, 3))
            return false;

        gyro_regs = Arrays.copyOf(gyro_sign, 3);
        accel_regs = Arrays.copyOf(accel_sign, 3);
        if ((orient & 4) != 0) {
            gyro_regs[0] |= 1;
            accel_regs[0] |= 1;
        }
        if ((orient & 0x20) != 0) {
            gyro_regs[1] |= 1;
            accel_regs[1] |= 1;
        }
        if ((orient & 0x100) != 0) {
            gyro_regs[2] |= 1;
            accel_regs[2] |= 1;
        }

        /* Chip-to-body, sign only. */
        if (!invMpu.mpu_write_mem((short)FCFG_3, gyro_regs, 0, 3))
            return false;
        if (!invMpu.mpu_write_mem((short)FCFG_7, accel_regs, 0, 3))
            return false;
        dmp.orient = orient;
        return true;
    }

    /**
     *  @brief      Set DMP output rate.
     *  Only used when DMP is on.
     *  @param[in]  rate    Desired fifo rate (Hz).
     *  @return     0 if successful.
     */
    boolean dmp_set_fifo_rate(int rate)
    {
        if (rate > DMP_SAMPLE_RATE)
            return false;

        byte[] regs_end = {(byte)DmpKey.DINAFE, (byte)DmpKey.DINAF2, (byte)DmpKey.DINAAB,
            (byte)0xc4, (byte)DmpKey.DINAAA, (byte)DmpKey.DINAF1, (byte)DmpKey.DINADF,
            (byte)DmpKey.DINADF, (byte)0xBB, (byte)0xAF, (byte)DmpKey.DINADF, (byte)DmpKey.DINADF};

        short div = (short)(DMP_SAMPLE_RATE / rate - 1);
        byte[] tmp = new byte[2];
        ByteBuffer.wrap(tmp).order(ByteOrder.BIG_ENDIAN).asShortBuffer().put(div);
        assert tmp[0] == ((div >> 8) & 0xFF);
        assert tmp[1] == (div & 0xFF);

        if (!invMpu.mpu_write_mem((short)D_0_22, tmp, 0, 2))
            return false;
        if (!invMpu.mpu_write_mem((short)CFG_6, regs_end, 0, 12))
            return false;

        dmp.fifo_rate = rate;
        return true;
    }

    /**
     *  @brief      Set tap threshold for a specific axis.
     *  @param[in]  axis    1, 2, and 4 for XYZ accel, respectively.
     *  @param[in]  thresh  Tap threshold, in mg/ms.
     *  @return     0 if successful.
     */
    boolean dmp_set_tap_thresh(short axis, short thresh)
    {
        byte[] tmp = new byte[4];
        int[] accel_fsr = new int[1];
        float scaled_thresh;
        int dmp_thresh, dmp_thresh_2;
        if (((axis & TAP_XYZ) == 0) || thresh > 1600)
            return false;

        scaled_thresh = (float)thresh / DMP_SAMPLE_RATE;

        invMpu.mpu_get_accel_fsr(accel_fsr);
        switch (accel_fsr[0]) {
        case 2:
            dmp_thresh = (int) scaled_thresh * 16384;
            /* dmp_thresh * 0.75 */
            dmp_thresh_2 = (int) scaled_thresh * 12288;
            break;
        case 4:
            dmp_thresh = (int) scaled_thresh * 8192;
            /* dmp_thresh * 0.75 */
            dmp_thresh_2 = (int) scaled_thresh * 6144;
            break;
        case 8:
            dmp_thresh = (int) scaled_thresh * 4096;
            /* dmp_thresh * 0.75 */
            dmp_thresh_2 = (int) scaled_thresh * 3072;
            break;
        case 16:
            dmp_thresh = (int) scaled_thresh * 2048;
            /* dmp_thresh * 0.75 */
            dmp_thresh_2 = (int) scaled_thresh * 1536;
            break;
        default:
            return false;
        }
        tmp[0] = (byte)(dmp_thresh >> 8);
        tmp[1] = (byte)(dmp_thresh & 0xFF);
        tmp[2] = (byte)(dmp_thresh_2 >> 8);
        tmp[3] = (byte)(dmp_thresh_2 & 0xFF);

        if ((axis & TAP_X) != 0) {
            if (!invMpu.mpu_write_mem((short)DmpMap.DMP_TAP_THX, tmp, 0, 2))
                return false;
            if (!invMpu.mpu_write_mem((short)D_1_36, tmp, 2, 2))
                return false;
        }
        if ((axis & TAP_Y) != 0) {
            if (!invMpu.mpu_write_mem((short)DmpMap.DMP_TAP_THY, tmp, 0, 2))
                return false;
            if (!invMpu.mpu_write_mem((short)D_1_40, tmp, 2, 2))
                return false;
        }
        if ((axis & TAP_Z) != 0) {
            if (!invMpu.mpu_write_mem((short)DmpMap.DMP_TAP_THZ, tmp, 0, 2))
                return false;
            if (!invMpu.mpu_write_mem((short)D_1_44, tmp, 2, 2))
                return false;
        }
        return true;
    }

    /**
     *  @brief      Set which axes will register a tap.
     *  @param[in]  axis    1, 2, and 4 for XYZ, respectively.
     *  @return     0 if successful.
     */
    boolean dmp_set_tap_axes(short axis)
    {
        byte[] tmp = new byte[1];
        tmp[0] = 0;

        if ((axis & TAP_X) != 0)
            tmp[0] |= 0x30;
        if ((axis & TAP_Y) != 0)
            tmp[0] |= 0x0C;
        if ((axis & TAP_Z) != 0)
            tmp[0] |= 0x03;
        return invMpu.mpu_write_mem((short)D_1_72, tmp, 0, 1);
    }

    /**
     *  @brief      Set minimum number of taps needed for an interrupt.
     *  @param[in]  min_taps    Minimum consecutive taps (1-4).
     *  @return     0 if successful.
     */
    boolean dmp_set_tap_count(short min_taps)
    {
        byte[] tmp = new byte[1];

        if (min_taps < 1)
            min_taps = 1;
        else if (min_taps > 4)
            min_taps = 4;

        tmp[0] = (byte) (min_taps - 1);
        return invMpu.mpu_write_mem((short)D_1_79, tmp, 0, 1);
    }

    /**
     *  @brief      Set length between valid taps.
     *  @param[in]  time    Milliseconds between taps.
     *  @return     0 if successful.
     */
    boolean dmp_set_tap_time(int time)
    {
        int dmp_time;
        byte[] tmp = new byte[2];

        dmp_time = time / (1000 / DMP_SAMPLE_RATE);
        tmp[0] = (byte)(dmp_time >> 8);
        tmp[1] = (byte)(dmp_time & 0xFF);
        return invMpu.mpu_write_mem((short)DmpMap.DMP_TAPW_MIN, tmp, 0, 2);
    }

    /**
     *  @brief      Set max time between taps to register as a multi-tap.
     *  @param[in]  time    Max milliseconds between taps.
     *  @return     0 if successful.
     */
        boolean dmp_set_tap_time_multi(int time)
    {
        int dmp_time;
        byte[] tmp = new byte[2];

        dmp_time = time / (1000 / DMP_SAMPLE_RATE);
        tmp[0] = (byte)(dmp_time >> 8);
        tmp[1] = (byte)(dmp_time & 0xFF);
        return invMpu.mpu_write_mem((short)D_1_218, tmp, 0, 2);
    }

    /**
     *  @brief      Set shake rejection threshold.
     *  If the DMP detects a gyro sample larger than @e thresh, taps are rejected.
     *  @param[in]  sf      Gyro scale factor.
     *  @param[in]  thresh  Gyro threshold in dps.
     *  @return     0 if successful.
     */
    boolean dmp_set_shake_reject_thresh(long sf, int thresh)
    {
        byte[] tmp = new byte[4];
        long thresh_scaled = sf / 1000 * thresh;
        tmp[0] = (byte)(((long)thresh_scaled >> 24) & 0xFF);
        tmp[1] = (byte)(((long)thresh_scaled >> 16) & 0xFF);
        tmp[2] = (byte)(((long)thresh_scaled >> 8) & 0xFF);
        tmp[3] = (byte)((long)thresh_scaled & 0xFF);
        return invMpu.mpu_write_mem((short)D_1_92, tmp, 0, 4);
    }

    /**
     *  @brief      Set shake rejection time.
     *  Sets the length of time that the gyro must be outside of the threshold set
     *  by @e gyro_set_shake_reject_thresh before taps are rejected. A mandatory
     *  60 ms is added to this parameter.
     *  @param[in]  time    Time in milliseconds.
     *  @return     0 if successful.
     */
    boolean dmp_set_shake_reject_time(int time)
    {
        byte[] tmp = new byte[2];

        time /= (1000 / DMP_SAMPLE_RATE);
        tmp[0] = (byte) (time >> 8);
        tmp[1] = (byte) (time & 0xFF);
        return invMpu.mpu_write_mem((short)D_1_90, tmp, 0, 2);
    }

    /**
     *  @brief      Set shake rejection timeout.
     *  Sets the length of time after a shake rejection that the gyro must stay
     *  inside of the threshold before taps can be detected again. A mandatory
     *  60 ms is added to this parameter.
     *  @param[in]  time    Time in milliseconds.
     *  @return     0 if successful.
     */
    boolean dmp_set_shake_reject_timeout(int time)
    {
        byte[] tmp = new byte[2];

        time /= (1000 / DMP_SAMPLE_RATE);
        tmp[0] = (byte) (time >> 8);
        tmp[1] = (byte) (time & 0xFF);
        return invMpu.mpu_write_mem((short)D_1_88, tmp, 0, 2);
    }

    /**
     *  @brief      Enable DMP features.
     *  The following \#define's are used in the input mask:
     *  \n DMP_FEATURE_TAP
     *  \n DMP_FEATURE_ANDROID_ORIENT
     *  \n DMP_FEATURE_LP_QUAT
     *  \n DMP_FEATURE_6X_LP_QUAT
     *  \n DMP_FEATURE_GYRO_CAL
     *  \n DMP_FEATURE_SEND_RAW_ACCEL
     *  \n DMP_FEATURE_SEND_RAW_GYRO
     *  \n NOTE: DMP_FEATURE_LP_QUAT and DMP_FEATURE_6X_LP_QUAT are mutually
     *  exclusive.
     *  \n NOTE: DMP_FEATURE_SEND_RAW_GYRO and DMP_FEATURE_SEND_CAL_GYRO are also
     *  mutually exclusive.
     *  @param[in]  mask    Mask of features to enable.
     *  @return     0 if successful.
     */
    boolean dmp_enable_feature(int mask)
    {
        byte[] tmp = new byte[10];

        /* TODO: All of these settings can probably be integrated into the default
         * DMP image.
         */
        /* Set integration scale factor. */
        ByteBuffer.wrap(tmp).order(ByteOrder.BIG_ENDIAN).asIntBuffer().put(GYRO_SF);
        assert tmp[0] == (byte)((GYRO_SF >> 24) & 0xFF);
        assert tmp[1] == (byte)((GYRO_SF >> 16) & 0xFF);
        assert tmp[2] == (byte)((GYRO_SF >> 8) & 0xFF);
        assert tmp[3] == (byte)(GYRO_SF & 0xFF);

        invMpu.mpu_write_mem((short)D_0_104, tmp, 0, 4);

        /* Send sensor data to the FIFO. */
        tmp[0] = (byte)0xA3;
        if ((mask & DMP_FEATURE_SEND_RAW_ACCEL) != 0) {
            tmp[1] = (byte)0xC0;
            tmp[2] = (byte)0xC8;
            tmp[3] = (byte)0xC2;
        } else {
            tmp[1] = (byte)0xA3;
            tmp[2] = (byte)0xA3;
            tmp[3] = (byte)0xA3;
        }
        if ((mask & DMP_FEATURE_SEND_ANY_GYRO) != 0) {
            tmp[4] = (byte)0xC4;
            tmp[5] = (byte)0xCC;
            tmp[6] = (byte)0xC6;
        } else {
            tmp[4] = (byte)0xA3;
            tmp[5] = (byte)0xA3;
            tmp[6] = (byte)0xA3;
        }
        tmp[7] = (byte)0xA3;
        tmp[8] = (byte)0xA3;
        tmp[9] = (byte)0xA3;
        invMpu.mpu_write_mem((short)CFG_15, tmp, 0, 10);

        /* Send gesture data to the FIFO. */
        if ((mask & (DMP_FEATURE_TAP | DMP_FEATURE_ANDROID_ORIENT)) != 0)
            tmp[0] = DmpKey.DINA20;
        else
            tmp[0] = (byte)0xD8;
        invMpu.mpu_write_mem((short)CFG_27, tmp, 0, 1);

        if ((mask & DMP_FEATURE_GYRO_CAL) != 0)
            dmp_enable_gyro_cal(true);
        else
            dmp_enable_gyro_cal(false);

        if ((mask & DMP_FEATURE_SEND_ANY_GYRO) != 0) {
            if ((mask & DMP_FEATURE_SEND_CAL_GYRO) != 0) {
                tmp[0] = (byte)0xB2;
                tmp[1] = (byte)0x8B;
                tmp[2] = (byte)0xB6;
                tmp[3] = (byte)0x9B;
            } else {
                tmp[0] = (byte)DmpKey.DINAC0;
                tmp[1] = (byte)DmpKey.DINA80;
                tmp[2] = (byte)DmpKey.DINAC2;
                tmp[3] = (byte)DmpKey.DINA90;
            }
            invMpu.mpu_write_mem((short)CFG_GYRO_RAW_DATA, tmp, 0, 4);
        }


        if ((mask & DMP_FEATURE_TAP) != 0) {
            /* Enable tap. */
            tmp[0] = (byte)0xF8;
            invMpu.mpu_write_mem((short)CFG_20, tmp, 0, 1);
            dmp_set_tap_thresh(TAP_XYZ, (short)250);
            dmp_set_tap_axes(TAP_XYZ);
            dmp_set_tap_count((short)1);
            dmp_set_tap_time(100);
            dmp_set_tap_time_multi(500);

            dmp_set_shake_reject_thresh(GYRO_SF, 200);
            dmp_set_shake_reject_time(40);
            dmp_set_shake_reject_timeout(10);
        } else {
            tmp[0] = (byte)0xD8;
            invMpu.mpu_write_mem((short)CFG_20, tmp, 0, 1);
        }

        if ((mask & DMP_FEATURE_ANDROID_ORIENT) != 0) {
            tmp[0] = (byte)0xD9;
        } else
            tmp[0] = (byte)0xD8;
        invMpu.mpu_write_mem((short)CFG_ANDROID_ORIENT_INT, tmp, 0, 1);

        if ((mask & DMP_FEATURE_LP_QUAT) != 0)
            dmp_enable_lp_quat(true);
        else
            dmp_enable_lp_quat(false);

        if ((mask & DMP_FEATURE_6X_LP_QUAT) != 0)
            dmp_enable_6x_lp_quat(true);
        else
            dmp_enable_6x_lp_quat(false);

        /* Pedometer is always enabled. */
        dmp.feature_mask = mask | DMP_FEATURE_PEDOMETER;
        invMpu.mpu_reset_fifo();

        dmp.packet_length = 0;
        if ((mask & DMP_FEATURE_SEND_RAW_ACCEL) != 0)
            dmp.packet_length += 6;
        if ((mask & DMP_FEATURE_SEND_ANY_GYRO) != 0)
            dmp.packet_length += 6;
        if ((mask & (DMP_FEATURE_LP_QUAT | DMP_FEATURE_6X_LP_QUAT)) != 0)
            dmp.packet_length += 16;
        if ((mask & (DMP_FEATURE_TAP | DMP_FEATURE_ANDROID_ORIENT)) != 0)
            dmp.packet_length += 4;

        return true;
    }

    /**
     *  @brief      Calibrate the gyro data in the DMP.
     *  After eight seconds of no motion, the DMP will compute gyro biases and
     *  subtract them from the quaternion output. If @e dmp_enable_feature is
     *  called with @e DMP_FEATURE_SEND_CAL_GYRO, the biases will also be
     *  subtracted from the gyro output.
     *  @param[in]  enable  1 to enable gyro calibration.
     *  @return     0 if successful.
     */
    boolean dmp_enable_gyro_cal(boolean enable)
    {
        if (enable) {
            byte[] regs = {(byte)0xb8, (byte)0xaa, (byte)0xb3, (byte)0x8d, (byte)0xb4, (byte)0x98, (byte)0x0d, (byte)0x35, (byte)0x5d};
            return invMpu.mpu_write_mem((short)CFG_MOTION_BIAS, regs, 0, 9);
        } else {
            byte[] regs = {(byte)0xb8, (byte)0xaa, (byte)0xaa, (byte)0xaa, (byte)0xb0, (byte)0x88, (byte)0xc3, (byte)0xc5, (byte)0xc7};
            return invMpu.mpu_write_mem((short)CFG_MOTION_BIAS, regs, 0, 9);
        }
    }

    /**
     *  @brief      Generate 3-axis quaternions from the DMP.
     *  In this driver, the 3-axis and 6-axis DMP quaternion features are mutually
     *  exclusive.
     *  @param[in]  enable  1 to enable 3-axis quaternion.
     *  @return     0 if successful.
     */
    boolean dmp_enable_lp_quat(boolean enable)
    {
        byte[] regs = new byte[4];
        if (enable) {
            regs[0] = (byte)DmpKey.DINBC0;
            regs[1] = (byte)DmpKey.DINBC2;
            regs[2] = (byte)DmpKey.DINBC4;
            regs[3] = (byte)DmpKey.DINBC6;
        } else {
            regs[0] = (byte)0x8B;
            regs[1] = (byte)0x8B;
            regs[2] = (byte)0x8B;
            regs[3] = (byte)0x8B;
        }
        invMpu.mpu_write_mem((short)CFG_LP_QUAT, regs, 0, 4);

        return invMpu.mpu_reset_fifo();
    }

    /**
     *  @brief       Generate 6-axis quaternions from the DMP.
     *  In this driver, the 3-axis and 6-axis DMP quaternion features are mutually
     *  exclusive.
     *  @param[in]   enable  1 to enable 6-axis quaternion.
     *  @return      0 if successful.
     */
    boolean dmp_enable_6x_lp_quat(boolean enable)
    {
        byte[] regs = new byte[4];
        if (enable) {
            regs[0] = (byte)DmpKey.DINA20;
            regs[1] = (byte)DmpKey.DINA28;
            regs[2] = (byte)DmpKey.DINA30;
            regs[3] = (byte)DmpKey.DINA38;
        } else {
            regs[0] = (byte)0xA3;
            regs[1] = (byte)0xA3;
            regs[2] = (byte)0xA3;
            regs[3] = (byte)0xA3;
        }
        invMpu.mpu_write_mem((short)CFG_8, regs, 0, 4);

        return invMpu.mpu_reset_fifo();
    }

    /**
     *  @brief      Decode the four-byte gesture data and execute any callbacks.
     *  @param[in]  gesture Gesture data from DMP packet.
     *  @return     0 if successful.
     */
    static boolean decode_gesture(byte[] gesture)
    {
        short android_orient = (short) ((gesture[3] & 0xC0) & 0xFF);
        short tap = (short) ((0x3F & gesture[3]) & 0xFF);

//        System.out.println("decode_gesture " + gesture[1]);

        if ((gesture[1] & INT_SRC_TAP) != 0) {
            byte direction = (byte) (tap >> 3);
            byte count = (byte) ((tap % 8) + 1);
            System.out.println("direction = " + direction);
            System.out.println("count = " + count);
//            if (dmp.tap_cb)
//                dmp.tap_cb(direction, count);
        }

        if ((gesture[1] & INT_SRC_ANDROID_ORIENT) != 0) {
            System.out.println("android_orient = " + (android_orient>>6));
//            if (dmp.android_orient_cb)
//                dmp.android_orient_cb(android_orient >> 6);
        }

        return true;
    }

    /**
     *  @brief      Get one packet from the FIFO.
     *  If @e sensors does not contain a particular sensor, disregard the data
     *  returned to that pointer.
     *  \n @e sensors can contain a combination of the following flags:
     *  \n INV_X_GYRO, INV_Y_GYRO, INV_Z_GYRO
     *  \n INV_XYZ_GYRO
     *  \n INV_XYZ_ACCEL
     *  \n INV_WXYZ_QUAT
     *  \n If the FIFO has no new data, @e sensors will be zero.
     *  \n If the FIFO is disabled, @e sensors will be zero and this function will
     *  return a non-zero error code.
     *  @param[out] gyro        Gyro data in hardware units.
     *  @param[out] accel       Accel data in hardware units.
     *  @param[out] quat        3-axis quaternion data in hardware units.
     *  @param[out] timestamp   Timestamp in milliseconds.
     *  @param[out] sensors     Mask of sensors read from FIFO.
     *  @param[out] more        Number of remaining packets.
     *  @return     0 if successful.
     */
    boolean dmp_read_fifo(short[] gyro, short[] accel, int[] quat,
            long[] timestamp, short[] sensors, short[] more) {

        byte[] fifo_data = new byte[dmp.packet_length];
        short ii = 0;

        /* TODO: sensors[0] only changes when dmp_enable_feature is called. We can
         * cache this value and save some cycles.
         */
        sensors[0] = 0;

        /* Get a packet. */
        if (!invMpu.mpu_read_fifo_stream(fifo_data, more))
            return false;

        /* Parse DMP packet. */
        if ((dmp.feature_mask & (DMP_FEATURE_LP_QUAT | DMP_FEATURE_6X_LP_QUAT)) != 0) {
            if (quat.length != 4)
                return false;

            ByteBuffer.wrap(fifo_data, ii, 4*quat.length).order(ByteOrder.BIG_ENDIAN).asIntBuffer().get(quat); // 4 bytes in an int
            assert quat[0] == (((fifo_data[0]&0xFF) << 24) | ((fifo_data[1]&0xFF) << 16) | ((fifo_data[2]&0xFF) << 8) | (fifo_data[3]&0xFF));
            assert quat[1] == (((fifo_data[4]&0xFF) << 24) | ((fifo_data[5]&0xFF) << 16) | ((fifo_data[6]&0xFF) << 8) | (fifo_data[7]&0xFF));
            assert quat[2] == (((fifo_data[8]&0xFF) << 24) | ((fifo_data[9]&0xFF) << 16) | ((fifo_data[10]&0xFF) << 8) | (fifo_data[11]&0xFF));
            assert quat[3] == (((fifo_data[12]&0xFF) << 24) | ((fifo_data[13]&0xFF) << 16) | ((fifo_data[14]&0xFF) << 8) | (fifo_data[15]&0xFF));
            ii += 4*quat.length;
            /* We can detect a corrupted FIFO by monitoring the quaternion data and
             * ensuring that the magnitude is always normalized to one. This
             * shouldn't happen in normal operation, but if an I2C error occurs,
             * the FIFO reads might become misaligned.
             *
             * Let's start by scaling down the quaternion data to avoid long long
             * math.
             */
            int[] quat_q14 = new int[4];
            quat_q14[0] = quat[0] >> 16;
            quat_q14[1] = quat[1] >> 16;
            quat_q14[2] = quat[2] >> 16;
            quat_q14[3] = quat[3] >> 16;
            int quat_mag_sq = quat_q14[0] * quat_q14[0] + quat_q14[1] * quat_q14[1] +
                    quat_q14[2] * quat_q14[2] + quat_q14[3] * quat_q14[3];
            if ((quat_mag_sq < QUAT_MAG_SQ_MIN) ||
                    (quat_mag_sq > QUAT_MAG_SQ_MAX)) {
                /* Quaternion is outside of the acceptable threshold. */
                invMpu.mpu_reset_fifo();
                sensors[0] = 0;
                return false;
            }
            sensors[0] |= INV_WXYZ_QUAT;
        }

        if ((dmp.feature_mask & DMP_FEATURE_SEND_RAW_ACCEL) != 0) {
            if (accel.length != 3)
                return false;
            ByteBuffer.wrap(fifo_data, ii, 2*accel.length).order(ByteOrder.BIG_ENDIAN).asShortBuffer().get(accel); // 2 bytes in a short
            assert accel[0] == (short)(((fifo_data[ii+0]&0xFF) << 8) | (fifo_data[ii+1]&0xFF));
            assert accel[1] == (short)(((fifo_data[ii+2]&0xFF) << 8) | (fifo_data[ii+3]&0xFF));
            assert accel[2] == (short)(((fifo_data[ii+4]&0xFF) << 8) | (fifo_data[ii+5]&0xFF));
            ii += 2*accel.length;
            sensors[0] |= InvMpu.INV_XYZ_ACCEL;
        }

        if ((dmp.feature_mask & DMP_FEATURE_SEND_ANY_GYRO) != 0) {
            if (gyro.length != 3)
                return false;
            ByteBuffer.wrap(fifo_data, ii, 2*gyro.length).order(ByteOrder.BIG_ENDIAN).asShortBuffer().get(gyro); // 2 bytes in a short
            assert gyro[0] == (short)(((fifo_data[ii+0]&0xFF) << 8) | (fifo_data[ii+1]&0xFF));
            assert gyro[1] == (short)(((fifo_data[ii+2]&0xFF) << 8) | (fifo_data[ii+3]&0xFF));
            assert gyro[2] == (short)(((fifo_data[ii+4]&0xFF) << 8) | (fifo_data[ii+5]&0xFF));
            ii += 2*gyro.length;
            sensors[0] |= InvMpu.INV_XYZ_GYRO;
        }

        /* Gesture data is at the end of the DMP packet. Parse it and call
         * the gesture callbacks (if registered).
         */
        if ((dmp.feature_mask & (DMP_FEATURE_TAP | DMP_FEATURE_ANDROID_ORIENT)) != 0) {
            decode_gesture(Arrays.copyOfRange(fifo_data, ii, ii+4));
        }

        if (timestamp != null && timestamp.length > 0)
            timestamp[0] = System.currentTimeMillis();
        return true;
    }
}
